#ifndef __BSP_SERVO_H__
#define __BSP_SERVO_H__

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include "Arduino.h"
#include <HardwareSerial.h>
#include <stdint.h>



#define SERVO_0  102 
#define SERVO_45  187 
#define SERVO_90  280 
#define SERVO_135  373 
#define SERVO_180  510 

#define SERVO_L 0
#define SERVO_R 1

#define LIFT 0
#define DOWN 1
#define CLIMB 2

#define SERVO_L_LIFT 113
#define SERVO_R_LIFT 471
#define SERVO_L_DOWN 317        
#define SERVO_R_DOWN 272
#define SERVO_L_CLIMB 351
#define SERVO_R_CLIMB 238


void Servo_Init(void);
void ServoRun(uint8_t LiftOrDown);

#endif
